/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package us.oh.k12.wkw.robot.command;

import edu.wpi.first.wpilibj.Utility;

/**
 *
 * @author Team4145
 */
public class AutonomousTurnCmd extends CommandBase {

    //private long startTime;
    //private long driveTime;
    //private double gyroAngle;
    //private double scaleFactor;

    public AutonomousTurnCmd() {
        super("AutonomousTurnCmd()");
        this.requires(this.getDriveSystem());
        //this.driveTime = 5 * 1000000;
        //this.setTimeout(10);
    }

    protected void initialize() {
        this.getDriveSystem().resetGyro();
        //this.startTime = Utility.getFPGATime();
    }

    protected void execute() {
        double aGyroAngle = this.getDriveSystem().getGyroHeading();
        double scale = 1 - Math.abs(aGyroAngle * 0.025);
        double aScaleFactor = (scale < .1) ? 0 : scale;
        this.getDriveSystem().tankDrive(0.5, 0.8 * aScaleFactor);
    }

    protected boolean isFinished() {
        return this.getDriveSystem().getGyroHeading() == 0;
    }

    protected void end() {
        this.getDriveSystem().tankDrive(0.0, 0.0);
    }
}
